import cv2
import numpy as np
import matplotlib.pyplot as plt
import math


def Translate(rawImg, xShift, yShift):
	rawHeight, rawWidth = rawImg.shape

	result = np.zeros([rawHeight, rawWidth], dtype=rawImg.dtype)

	for y in range(rawHeight):
		for x in range(rawWidth):
			mappedX = int(round(x - xShift))
			mappedY = int(round(y - yShift))

			if (mappedX < 0) or (rawWidth < mappedX) or (mappedY < 0) or (rawHeight < mappedY):
				continue

			result[y, x] = rawImg[int(round(mappedY)), int(round(mappedX))]

	return result


def TranslateWithBilinear(rawImg, xShift, yShift):
	rawHeight, rawWidth = rawImg.shape

	result = np.zeros([rawHeight, rawWidth], dtype=rawImg.dtype)

	for y in range(rawHeight):
		for x in range(rawWidth):
			mappedX = x - xShift
			mappedY = y - yShift

			if (mappedX < 0) or (rawWidth < mappedX) or (mappedY < 0) or (rawHeight < mappedY):
				continue

			mappedXCeil = math.ceil(mappedX)
			mappedXFloor = math.floor(mappedX)
			mappedYCeil = math.ceil(mappedY)
			mappedYFloor = math.floor(mappedY)

			if (mappedXFloor < 0) or (rawWidth <= mappedXCeil) or (mappedYFloor < 0) or (rawHeight <= mappedYCeil):
				continue

			if mappedXCeil == mappedXFloor:
				if mappedYCeil == mappedYFloor:
					result[y, x] = rawImg[mappedY, mappedX]
				else:
					result[y, x] = (mappedYCeil - mappedY) * rawImg[mappedYFloor, mappedX] + (mappedY - mappedYFloor) * rawImg[mappedYCeil, mappedX]
			else:
				if mappedYCeil == mappedYFloor:
					result[y, x] = (mappedXCeil - mappedX) * rawImg[mappedY, mappedXFloor] + (mappedX - mappedXFloor) * rawImg[mappedY, mappedXCeil]
				else:
					YCeilAvg = (mappedX - mappedXFloor)*rawImg[mappedYCeil, mappedXCeil] + (mappedXCeil - mappedX)*rawImg[mappedYCeil, mappedXFloor]
					YFloorAvg = (mappedX - mappedXFloor)*rawImg[mappedYFloor, mappedXCeil] + (mappedXCeil - mappedX)*rawImg[mappedYFloor, mappedXFloor]

					result[y, x] = (mappedY - mappedYFloor) * YCeilAvg + (mappedYCeil - mappedY) * YFloorAvg

	return result


def RotateWithBilinear(rawImg, angle):
	rawHeight, rawWidth = rawImg.shape

	result = np.zeros([rawHeight, rawWidth], dtype=rawImg.dtype)

	anglePi = angle*math.pi/180.0
	cosA = math.cos(anglePi)
	sinA = math.sin(anglePi)

	for y in range(rawHeight):
		for x in range(rawWidth):
			mappedY = cosA*y-sinA*x-0.5*rawWidth*cosA+0.5*rawHeight*sinA+0.5*rawWidth
			mappedX = sinA*y+cosA*x-0.5*rawWidth*sinA-0.5*rawHeight*cosA+0.5*rawHeight

			if (0 <= mappedX) and (mappedX < rawWidth) and (0 <= mappedY) and (mappedY < rawHeight):
				result[y, x] = rawImg[int(mappedY), int(mappedX)]


			mappedXCeil = math.ceil(mappedX)
			if (rawWidth <= mappedXCeil):
				continue

			mappedXFloor = math.floor(mappedX)
			if (mappedXFloor < 0):
				continue
			
			mappedYCeil = math.ceil(mappedY)
			if (rawHeight <= mappedYCeil):
				continue

			mappedYFloor = math.floor(mappedY)
			if (mappedYFloor < 0):
				continue

			if mappedXCeil == mappedXFloor:
				if mappedYCeil == mappedYFloor:
					result[y, x] = rawImg[mappedY, mappedX]
				else:
					result[y, x] = (mappedYCeil - mappedY) * rawImg[mappedYFloor, mappedX] + (mappedY - mappedYFloor) * rawImg[mappedYCeil, mappedX]
			else:
				if mappedYCeil == mappedYFloor:
					result[y, x] = (mappedXCeil - mappedX) * rawImg[mappedY, mappedXFloor] + (mappedX - mappedXFloor) * rawImg[mappedY, mappedXCeil]
				else:
					YCeilAvg = (mappedX - mappedXFloor)*rawImg[mappedYCeil, mappedXCeil] + (mappedXCeil - mappedX)*rawImg[mappedYCeil, mappedXFloor]
					YFloorAvg = (mappedX - mappedXFloor)*rawImg[mappedYFloor, mappedXCeil] + (mappedXCeil - mappedX)*rawImg[mappedYFloor, mappedXFloor]

					result[y, x] = (mappedY - mappedYFloor) * YCeilAvg + (mappedYCeil - mappedY) * YFloorAvg


	return result


def RotateWithNN(rawImg, angle):
	rawHeight, rawWidth = rawImg.shape

	result = np.zeros([rawHeight, rawWidth], dtype=rawImg.dtype)

	anglePi = angle*math.pi/180.0
	cosA = math.cos(anglePi)
	sinA = math.sin(anglePi)

	for y in range(rawHeight):
		for x in range(rawWidth):
			mappedY = round(cosA*y-sinA*x-0.5*rawWidth*cosA+0.5*rawHeight*sinA+0.5*rawWidth)
			mappedX = round(sinA*y+cosA*x-0.5*rawWidth*sinA-0.5*rawHeight*cosA+0.5*rawHeight)

			if (0 <= mappedX) and (mappedX < rawWidth) and (0 <= mappedY) and (mappedY < rawHeight):
				result[y, x] = rawImg[int(mappedY), int(mappedX)]

	return result


def PositionRemap(newP, newRange, oldRange):
	return newP*((oldRange-1) * 1.0 /(newRange-1))


def ScaleWithBilinear(rawImg, saclefactor):
	rawHeight, rawWidth = rawImg.shape
	newHeight = int(round(rawHeight * saclefactor))
	newWidth = int(round(rawWidth * saclefactor))

	result = np.zeros([newHeight, newWidth], dtype=rawImg.dtype)

	for y in range(newHeight):
		for x in range(newWidth):
			mappedX = PositionRemap(x, newWidth, rawWidth)
			mappedY = PositionRemap(y, newHeight, rawHeight)
			mappedXCeil = math.ceil(mappedX)
			mappedXFloor = math.floor(mappedX)
			mappedYCeil = math.ceil(mappedY)
			mappedYFloor = math.floor(mappedY)

			if mappedXCeil == mappedXFloor:
				if mappedYCeil == mappedYFloor:
					result[y, x] = rawImg[mappedY, mappedX]
				else:
					result[y, x] = (mappedYCeil - mappedY) * rawImg[mappedYFloor, mappedX] + (mappedY - mappedYFloor) * rawImg[mappedYCeil, mappedX]
			else:
				if mappedYCeil == mappedYFloor:
					result[y, x] = (mappedXCeil - mappedX) * rawImg[mappedY, mappedXFloor] + (mappedX - mappedXFloor) * rawImg[mappedY, mappedXCeil]
				else:
					YCeilAvg = (mappedX - mappedXFloor)*rawImg[mappedYCeil, mappedXCeil] + (mappedXCeil - mappedX)*rawImg[mappedYCeil, mappedXFloor]
					YFloorAvg = (mappedX - mappedXFloor)*rawImg[mappedYFloor, mappedXCeil] + (mappedXCeil - mappedX)*rawImg[mappedYFloor, mappedXFloor]

					result[y, x] = (mappedY - mappedYFloor) * YCeilAvg + (mappedYCeil - mappedY) * YFloorAvg

	return result


def ScaleWithNN(rawImg, saclefactor):
	rawHeight, rawWidth = rawImg.shape
	newHeight = int(round(rawHeight * saclefactor))
	newWidth = int(round(rawWidth * saclefactor))

	result = np.zeros([newHeight, newWidth], dtype=rawImg.dtype)

	for y in range(newHeight):
		for x in range(newWidth):
			mappedX = PositionRemap(x, newWidth, rawWidth)
			mappedY = PositionRemap(y, newHeight, rawHeight)
			result[y, x] = rawImg[int(round(mappedY)), int(round(mappedX))]

	return result



if __name__ == '__main__':
	M = 500
	N = 750
	print PositionRemap(332, N, M)
	print PositionRemap(615, N, M)
	print "========================"
	rawImg = cv2.imread('./img/fig3.tif',0)
	cv2.namedWindow("Original Image", cv2.CV_WINDOW_AUTOSIZE)
	cv2.imshow("Original Image", rawImg)

	# NNResult = ScaleWithNN(rawImg, 0.5)
	# cv2.namedWindow("Image Scalled 0.5 by NN", cv2.CV_WINDOW_AUTOSIZE)
	# cv2.imshow("Image Scalled 0.5 by NN", NNResult)

	NNResult2 = ScaleWithNN(rawImg, 2.3)
	cv2.namedWindow("Image Scalled 2 by NN", cv2.CV_WINDOW_AUTOSIZE)
	cv2.imshow("Image Scalled 2 by NN", NNResult2)

	# BiResult = ScaleWithBilinear(rawImg, 0.5)
	# cv2.namedWindow("Image Scalled 0.5 by Bilinear", cv2.CV_WINDOW_AUTOSIZE)
	# cv2.imshow("Image Scalled 0.5 by Bilinear", BiResult)

	BiResult2 = ScaleWithBilinear(rawImg, 2.3)
	cv2.namedWindow("Image Scalled 2.3 by Bilinear", cv2.CV_WINDOW_AUTOSIZE)
	cv2.imshow("Image Scalled 2.3 by Bilinear", BiResult2)

	# rawHeight, rawWidth = rawImg.shape
	# newHeight = int(round(rawHeight * 2.3))
	# newWidth = int(round(rawWidth * 2.3))
	# BiResult = cv2.resize(rawImg, (newHeight, newWidth), interpolation =cv2.INTER_LINEAR)
	# cv2.namedWindow("CV Scalled 2 by Bilinear", cv2.CV_WINDOW_AUTOSIZE)
	# cv2.imshow("CV Scalled 2 by Bilinear", BiResult)

	print "========================"

	RotatedResult = RotateWithNN(BiResult2, 15)
	cv2.namedWindow("Image Rotated 45 by NN", cv2.CV_WINDOW_AUTOSIZE)
	cv2.imshow("Image Rotated 45 by NN", RotatedResult)

	RotatedResult2 = RotateWithBilinear(BiResult2, 15)
	cv2.namedWindow("Image Rotated 45 by Bilinear", cv2.CV_WINDOW_AUTOSIZE)
	cv2.imshow("Image Rotated 45 by Bilinear", RotatedResult2)

	print "========================"

	ShiftedResult = Translate(rawImg, 10.2, 10.2)
	cv2.namedWindow("Image Shifted right 10.2 by NN", cv2.CV_WINDOW_AUTOSIZE)
	cv2.imshow("Image Shifted right 10.2 by NN", ShiftedResult)

	ShiftedResultBi = TranslateWithBilinear(rawImg, 10.2, 10.2)
	cv2.namedWindow("Image Shifted right 10.2 by Bilinear", cv2.CV_WINDOW_AUTOSIZE)
	cv2.imshow("Image Shifted right 10.2 by Bilinear", ShiftedResultBi)
	

	plt.show()
	
	cv2.waitKey(0)
	cv2.destroyAllWindows()